﻿using System;
using System.Collections.Generic;
using System.Linq;
using System.Text;

namespace Effects.Application
{
    /// <summary>
    ///  Help class. It is used while developing new filters.
    /// </summary>
    class FilterTesting
    {

        public static void Compute_2nd_order(double frequency, int sampleRate)
        {
            Console.WriteLine("2nd order Low Pass - " + frequency + "Hz");

            System.Globalization.CultureInfo currenteInfo = System.Threading.Thread.CurrentThread.CurrentCulture;

            System.Threading.Thread.CurrentThread.CurrentCulture = new System.Globalization.CultureInfo("en-GB");

            double omega = frequency / sampleRate * 2 * Math.PI;
            double K = Math.Tan(omega / 2);
            double K2 = K * K;


            double[] a_test = new double[3];
            double[] b_test = new double[3];

            a_test[0] = K2;
            a_test[1] = 2 * K2;
            a_test[2] = K2;

            b_test[0] = 1 + Math.Sqrt(2) * K + K2;
            b_test[1] = 2 * (K2 - 1);
            b_test[2] = (K2 - Math.Sqrt(2) * K + 1);

            // ------------
            // normalized
            for (int i = 0; i < 3; i++)
                a_test[i] /= b_test[0];

            for (int i = 1; i < 3; i++)
                b_test[i] /= b_test[0];

            b_test[0] = 1;
            // ------------

            Console.WriteLine("freqz([" + a_test[0] + "," + a_test[1] + "," + a_test[2] + "],[" + b_test[0] + "," + b_test[1] + "," + b_test[2] + "]);");
            System.Threading.Thread.CurrentThread.CurrentCulture = currenteInfo ;
        }

        public static void Compute_2nd_order_highPass(double frequency, int sampleRate)
        {
            Console.WriteLine("2nd order High Pass - " + frequency + "Hz");

            System.Globalization.CultureInfo currenteInfo = System.Threading.Thread.CurrentThread.CurrentCulture;

            System.Threading.Thread.CurrentThread.CurrentCulture = new System.Globalization.CultureInfo("en-GB");

            // normalize frequency
            double omega = (sampleRate / 2 - frequency) / sampleRate * 2 * Math.PI;

            // pre-warp frequency
            double K = Math.Tan(omega / 2);
            double K2 = K * K;

            double[] a_test = new double[3];
            double[] b_test = new double[3];

            a_test[0] = K2;
            a_test[1] = -2 * K2;
            a_test[2] = K2;

            b_test[0] = 1 + Math.Sqrt(2) * K + K2;
            b_test[1] = -2 * (K2 - 1);
            b_test[2] = (K2 - Math.Sqrt(2) * K + 1);

            // ------------
            // normalized
            for (int i = 0; i < 3; i++)
                a_test[i] /= b_test[0];

            for (int i = 1; i < 3; i++)
                b_test[i] /= b_test[0];
            b_test[0] = 1;

            Console.WriteLine("freqz([" + a_test[0] + "," + a_test[1] + "," + a_test[2] + "],[" + b_test[0] + "," + b_test[1] + "," + b_test[2] + "]);");
            System.Threading.Thread.CurrentThread.CurrentCulture = currenteInfo;
        }

        public static void Compute_2nd_order_bandPass(double freqLow, double freqHigh, int sampleRate)
        {
            Console.WriteLine("2nd order Band Pass -  low:" + freqLow + "Hz, up: " + freqHigh + "Hz");
            System.Globalization.CultureInfo currenteInfo = System.Threading.Thread.CurrentThread.CurrentCulture;
            System.Threading.Thread.CurrentThread.CurrentCulture = new System.Globalization.CultureInfo("en-GB");

            // normalize frequencies
            double normalizedLow = freqLow / sampleRate * 2 * Math.PI;
            double normalizedHigh = freqHigh / sampleRate * 2 * Math.PI;

            // pre-warp frequencies
            double omegaLow = Math.Tan(normalizedLow / 2);
            double omegaHigh = Math.Tan(normalizedHigh / 2);

            double A = omegaHigh - omegaLow;
            double B = omegaLow * omegaHigh;

            double[] a = new double[3];
            double[] b = new double[3];

            a[0] = A;
            a[1] = 0;
            a[2] = -A;

            b[0] = 1 + A + B;
            b[1] = 2 *( B - 1);
            b[2] = 1 + B - A;

            // ------------
            // normalized
            for (int i = 0; i < 3; i++)
                a[i] /= b[0];

            for (int i = 1; i < 3; i++)
                b[i] /= b[0];

            b[0] = 1;
            // ------------


            Console.Write("freqz([");
            for (int i = 0; i < 2; i++)
                Console.Write(a[i] + ",");
            Console.Write(a[2] + "],[");

            for (int i = 0; i < 2; i++)
                Console.Write(b[i] + ",");
            Console.Write(b[2] + "],8192," + sampleRate + ");");
            Console.WriteLine();

            System.Threading.Thread.CurrentThread.CurrentCulture = currenteInfo;

        }
        
        public static void Compute_2nd_order_bandStop(double freqLow, double freqHigh, int sampleRate)
        {
            Console.WriteLine("2nd order Band Pass -  low:" + freqLow + "Hz, up: " + freqHigh + "Hz");
            System.Globalization.CultureInfo currenteInfo = System.Threading.Thread.CurrentThread.CurrentCulture;
            System.Threading.Thread.CurrentThread.CurrentCulture = new System.Globalization.CultureInfo("en-GB");

            // normalize frequencies
            double normalizedLow = freqLow / sampleRate * 2 * Math.PI;
            double normalizedHigh = freqHigh / sampleRate * 2 * Math.PI;

            // pre-warp frequencies
            double omegaLow = Math.Tan(normalizedLow / 2);
            double omegaHigh = Math.Tan(normalizedHigh / 2);

            double A = omegaHigh - omegaLow;
            double B = omegaLow * omegaHigh;

            double[] a = new double[3];
            double[] b = new double[3];

            a[0] = B+1;
            a[1] = 2*(B-1);
            a[2] = B+1;

            b[0] = 1 + A + B;
            b[1] = 2 * (B - 1);
            b[2] = 1 + B - A;

            // ------------
            // normalized
            for (int i = 0; i < 3; i++)
                a[i] /= b[0];

            for (int i = 1; i < 3; i++)
                b[i] /= b[0];

            b[0] = 1;
            // ------------


            Console.Write("freqz([");
            for (int i = 0; i < 2; i++)
                Console.Write(a[i] + ",");
            Console.Write(a[2] + "],[");

            for (int i = 0; i < 2; i++)
                Console.Write(b[i] + ",");
            Console.Write(b[2] + "],8192," + sampleRate + ");");
            Console.WriteLine();

            System.Threading.Thread.CurrentThread.CurrentCulture = currenteInfo;

        }


        public static void Compute_4th_order(double frequency, int sampleRate)
        {
            Console.WriteLine("4th order Low Pass - " + frequency + "Hz");

            System.Globalization.CultureInfo currenteInfo = System.Threading.Thread.CurrentThread.CurrentCulture;

            System.Threading.Thread.CurrentThread.CurrentCulture = new System.Globalization.CultureInfo("en-GB");

            double warp = frequency / sampleRate * Math.PI * 2;
            double omega = Math.Tan(warp / 2);

            //double omega = Math.Tan(frequency / sampleRate /2);
            double omega2 = omega * omega;
            double omega4 = omega2 * omega2;

            double A = 0.7654;
            double B = 1.8478;

            double X1 = omega2 + 1 - A * omega;
            double X2 = 2 * (omega2 - 1);
            double X3 = omega2 + 1 + A * omega;

            double Y1 = omega2 + 1 - B * omega;
            double Y2 = 2 * (omega2 - 1);
            double Y3 = omega2 + 1 + B * omega;

            double[] a = new double[5];
            double[] b = new double[5];

            a[0] = 1 * omega4;
            a[1] = 4 * omega4;
            a[2] = 6 * omega4;
            a[3] = 4 * omega4;
            a[4] = 1 * omega4;

            b[0] = X3 * Y3;
            b[1] = X2 * Y3 + X3 * Y2;
            b[2] = X1 * Y3 + X2 * Y2 + X3 * Y1;
            b[3] = X1 * Y2 + X2 * Y1;
            b[4] = X1 * Y1;

            // ------------
            // normalized
            for (int i = 0; i < 5; i++)
                a[i] /= b[0];

            for (int i = 1; i < 5; i++)
                b[i] /= b[0];

            b[0] = 1;
            // ------------

            Console.WriteLine("freqz([" + a[0] + "," + a[1] + "," + a[2] + "," + a[3] + "," + a[4] + "],[" + b[0] + "," + b[1] + "," + b[2] + "," + b[3] + "," + b[4] + "]);");
            System.Threading.Thread.CurrentThread.CurrentCulture = currenteInfo ;
        }

        public static void Compute_8th_order(double frequency, int sampleRate)
        {
            Console.WriteLine("8th order Low Pass - " + frequency + "Hz");
            System.Globalization.CultureInfo currenteInfo = System.Threading.Thread.CurrentThread.CurrentCulture;
            System.Threading.Thread.CurrentThread.CurrentCulture = new System.Globalization.CultureInfo("en-GB");

            double warp = frequency / sampleRate * Math.PI * 2;
            double omega = Math.Tan(warp / 2);

            double omega2 = omega * omega;
            double omega8 = Math.Pow(omega, 8);

            double I = 0.3902;
            double J = 1.1111;
            double K = 1.6629;
            double L = 1.9616; 

            double Common = 2 * (omega2 - 1);
            double Im = 1 - I * omega + omega2;
            double Ip = 1 + I * omega + omega2;
            double Jm = 1 - J * omega + omega2;
            double Jp = 1 + J * omega + omega2;
            double Km = 1 - K * omega + omega2;
            double Kp = 1 + K * omega + omega2;
            double Lm = 1 - L * omega + omega2;
            double Lp = 1 + L * omega + omega2;

            double X5 = Im * Jm;
            double X4 = Common * Jm + Common * Im;
            double X3 = Common * Common + Ip * Jm + Im * Jp;
            double X2 = Common * Ip + Common * Jp;
            double X1 = Ip * Jp;

            double Y5 = Km * Lm;
            double Y4 = Common * Lm + Common * Km;
            double Y3 = Common * Common + Kp * Lm + Km * Lp;
            double Y2 = Common * Kp + Common * Lp;
            double Y1 = Kp * Lp;

            double[] a = new double[9];
            double[] b = new double[9];

            a[0] = omega8;
            a[1] = omega8 * 8;
            a[2] = omega8 * 28;
            a[3] = omega8 * 56;
            a[4] = omega8 * 70;
            a[5] = omega8 * 56;
            a[6] = omega8 * 28;
            a[7] = omega8 * 8;
            a[8] = omega8;

            b[8] = X5 * Y5;
            b[7] = X4 * Y5 + X5 * Y4;
            b[6] = X3 * Y5 + X4 * Y4 + X5 * Y3;
            b[5] = X2 * Y5 + X3 * Y4 + X4 * Y3 + X5 * Y2;
            b[4] = X1 * Y5 + X2 * Y4 + X3 * Y3 + X4 * Y2 + X5 * Y1;
            b[3] = X1 * Y4 + X2 * Y3 + X3 * Y2 + X4 * Y1;
            b[2] = X1 * Y3 + X2 * Y2 + X3 * Y1;
            b[1] = X1 * Y2 + X2 * Y1;
            b[0] = X1 * Y1;


            // --------------------------
            // NORMALIZED 

            for (int i = 0; i < 9; i++)
                a[i] = a[i] / b[0];
            for (int i = 1; i < 9; i++)
                b[i] = b[i] / b[0];

            b[0] = 1;
            // -------------------------

            Console.Write("freqz([");
            for (int i = 0; i < 8; i++)
                Console.Write(a[i] + ",");
            Console.Write(a[8] + "],[");

            for (int i = 0; i < 8; i++)
                Console.Write(b[i] + ",");
            Console.Write(b[8] + "],512," + sampleRate + ");");
            Console.WriteLine();
            
            System.Threading.Thread.CurrentThread.CurrentCulture = currenteInfo;
        }

        public static void Compute_4th_order_peak(double lowFreq, double upFreq, int sampleRate,double gainDB)
        {
            Console.WriteLine("4th order Peak -  low:" + lowFreq + "Hz, up: " + upFreq +"Hz");
            System.Globalization.CultureInfo currenteInfo = System.Threading.Thread.CurrentThread.CurrentCulture;
            System.Threading.Thread.CurrentThread.CurrentCulture = new System.Globalization.CultureInfo("en-GB");

            double gain = Math.Pow(10, gainDB / 20) - 1;

            double normalizedLow = lowFreq / sampleRate * 2 * Math.PI;
            double normalizedUp = upFreq / sampleRate * 2 * Math.PI;

            double omegaLow = Math.Tan(normalizedLow / 2);
            double omegaUp = Math.Tan(normalizedUp / 2);

            double A = omegaLow * omegaUp;
            double B = omegaUp - omegaLow;

            double X1 = Math.Sqrt(2) * B;
            double X2 = 2 * A + B * B + gain * B * B;
            double X3 = X1 * A;
            double X4 = A * A;

            double Y1 = X1;
            double Y2 = 2 * A + B * B;
            double Y3 = X3;
            double Y4 = X4;

            double[] a = new double[5];
            double[] b = new double[5];

            a[0] = 1 + X1 + X2 + X3 + X4;
            a[1] = -4 - 2 * X1 + 2 * X3 + 4 * X4;
            a[2] = 6 - 2 * X2 + 6 * X4;
            a[3] = -4 + 2* X1 -2 * X3 + 4 * X4;
            a[4] = 1 - X1 + X2 - X3 + X4;

            b[0] = 1 + Y1 + Y2 + Y3 + Y4;
            b[1] = -4 - 2 * Y1 + 2 * Y3 + 4 * Y4;
            b[2] = 6 - 2 * Y2 + 6 * Y4;
            b[3] = -4 + 2 * Y1 - 2 * Y3 + 4 * Y4;
            b[4] = 1 - Y1 + Y2 - Y3 + Y4;

            Console.Write("freqz([");
            for (int i = 0; i < 4; i++)
                Console.Write(a[i] + ",");
            Console.Write(a[4] + "],[");

            for (int i = 0; i < 4; i++)
                Console.Write(b[i] + ",");
            Console.Write(b[4] + "],512," + sampleRate + ");");
            Console.WriteLine();

            System.Threading.Thread.CurrentThread.CurrentCulture = currenteInfo;
        }

        public static void Compute_4th_order_band_pass(double lowFreq, double upFreq, int sampleRate)
        {
            Console.WriteLine("4th order Band Pass -  low:" + lowFreq + "Hz, up: " + upFreq + "Hz");
            System.Globalization.CultureInfo currenteInfo = System.Threading.Thread.CurrentThread.CurrentCulture;
            System.Threading.Thread.CurrentThread.CurrentCulture = new System.Globalization.CultureInfo("en-GB");

            double normalizedLow = lowFreq / sampleRate * 2 * Math.PI;
            double normalizedUp = upFreq / sampleRate * 2 * Math.PI;

            double omegaLow = Math.Tan(normalizedLow / 2);
            double omegaUp = Math.Tan(normalizedUp / 2);

            double A = omegaLow * omegaUp;
            double B = omegaUp - omegaLow;

            double Y1 = Math.Sqrt(2) * B;
            double Y2 = 2 * A + B * B;
            double Y3 = Y1 * A;
            double Y4 = A * A;

            double[] a = new double[5];
            double[] b = new double[5];

            a[0] = B * B;
            a[1] = 0;
            a[2] = -2 * B * B;
            a[3] = 0;
            a[4] = a[0];

            b[0] = 1 + Y1 + Y2 + Y3 + Y4;
            b[1] = -4 - 2 * Y1 + 2 * Y3 + 4 * Y4;
            b[2] = 6 - 2 * Y2 + 6 * Y4;
            b[3] = -4 + 2 * Y1 - 2 * Y3 + 4 * Y4;
            b[4] = 1 - Y1 + Y2 - Y3 + Y4;

            // ------------
            // normalized
            for (int i = 0; i < 5; i++)
                a[i] /= b[0];

            for (int i = 1; i < 5; i++)
                b[i] /= b[0];

            b[0] = 1;
            // ------------


            Console.Write("freqz([");
            for (int i = 0; i < 4; i++)
                Console.Write(a[i] + ",");
            Console.Write(a[4] + "],[");

            for (int i = 0; i < 4; i++)
                Console.Write(b[i] + ",");
            Console.Write(b[4] + "],4096," + sampleRate + ");");
            Console.WriteLine();

            System.Threading.Thread.CurrentThread.CurrentCulture = currenteInfo;
        }

        public static void Compute_4th_order_high_pass(double frequency, int sampleRate)
        {
            Console.WriteLine("4th order High Pass - " + frequency + "Hz");

            System.Globalization.CultureInfo currenteInfo = System.Threading.Thread.CurrentThread.CurrentCulture;

            System.Threading.Thread.CurrentThread.CurrentCulture = new System.Globalization.CultureInfo("en-GB");

            double warp = (sampleRate/ 2 - frequency) / sampleRate * Math.PI * 2;
            double omega = Math.Tan(warp / 2);

            //double omega = Math.Tan(frequency / sampleRate /2);
            double omega2 = omega * omega;
            double omega4 = omega2 * omega2;

            double A = 0.7654;
            double B = 1.8478;

            double X1 = omega2 + 1 - A * omega;
            double X2 = 2 * (omega2 - 1);
            double X3 = omega2 + 1 + A * omega;

            double Y1 = omega2 + 1 - B * omega;
            double Y2 = 2 * (omega2 - 1);
            double Y3 = omega2 + 1 + B * omega;

            double[] a = new double[5];
            double[] b = new double[5];

            a[0] = 1 * omega4;
            a[1] = -4 * omega4;
            a[2] = 6 * omega4;
            a[3] = -4 * omega4;
            a[4] = 1 * omega4;

            b[0] = X3 * Y3;
            b[1] = -1*(X2 * Y3 + X3 * Y2);
            b[2] = X1 * Y3 + X2 * Y2 + X3 * Y1;
            b[3] = -1*(X1 * Y2 + X2 * Y1);
            b[4] = X1 * Y1;

            // ------------
            // normalized
            for (int i = 0; i < 5; i++)
                a[i] /= b[0];

            for (int i = 1; i < 5; i++)
                b[i] /= b[0];

            b[0] = 1;
            // ------------

            Console.WriteLine("freqz([" + a[0] + "," + a[1] + "," + a[2] + "," + a[3] + "," + a[4] + "],[" + b[0] + "," + b[1] + "," + b[2] + "," + b[3] + "," + b[4] + "]);");
            System.Threading.Thread.CurrentThread.CurrentCulture = currenteInfo;
        }

        
        public static void Main_method(string[] args)
        {
            //Compute_2nd_order(1000, 44100);
            //Compute_2nd_order_highPass(2000, 44100);
            //Compute_4th_order(1000, 44100);
            //Compute_8th_order(1000, 44100);

            //Compute_4th_order_band_pass(1000, 2000, 44100);
            Compute_2nd_order_bandPass(1000, 2000, 44100);
            Compute_2nd_order_bandStop(4000, 10000, 44100);
            
            Console.ReadLine();
        } 
    }
}
